#!/usr/bin/python3
# -*- coding: utf-8 -*-
# 开始编码格式和运行环境选择
from .base.controller2 import *
import math
import numpy as np
from threading import Thread
import yaml, os, sys

sys.path.append(os.path.abspath(os.path.join(os.path.dirname(__file__), ".."))) 
# 导入自定义log模块
from log_info import logger


def limit_val(val, min_val, max_val):
    return max(min(val, max_val), min_val)

# 编码值和真实单位的换算
class MotoConvert:
    def __init__(self, perimeter=None) -> None:
        # 编码器一圈的编码值
        self.encoder_resolution = 2016
        # 编码速度转换值
        self.speed_rate = 100
        if perimeter is None:
            perimeter = 0.06*math.pi
        self.dis_resolution = perimeter / self.encoder_resolution
     
    def set_perimeter(self, perimeter):
        self.dis_resolution = perimeter / self.encoder_resolution
        # print(self.dis_resolution)
    
    def set_diameter(self, diameter):
        self.dis_resolution = diameter * math.pi / self.encoder_resolution
    
    def sp2virtual(self, speed):
        # 速度转为encoder输出
        speed_encoder = speed / self.dis_resolution
        # encoder转为控制器设置值
        speed_out = int(speed_encoder / self.speed_rate)
        speed_out = limit_val(speed_out, -100, 100)
        return int(speed_out)
    
    def dis2true(self, encoder_dis):
        dis_out = encoder_dis * self.dis_resolution
        return dis_out
    
    def sp2true(self, speed):
        # 控制器速度转为encoder输出
        speed_encoder = int(speed * self.speed_rate)
        speed_out = speed_encoder * self.dis_resolution
        return speed_out
    
    def encoder2dis(self, encoder_dis):
        dis_out = encoder_dis * self.dis_resolution
        return dis_out
    
    def dis2encoder(self, dis):
        encoder_out = dis / self.dis_resolution
        return encoder_out

class MecanumBase():
    def __init__(self) -> None:
        logger.info("mecanum init")
        # 配置文件存储路径
        self.yaml_name = "config_vehicel.yaml"
        self.path_dir = os.path.abspath(os.path.dirname(__file__))
        self.yaml_path = os.path.join(self.path_dir, self.yaml_name)
        # 根据路径获取配置信息
        self.get_yaml_config()
        self.buzzer = Beep()
        self.base_moto = Motor4()

        self.encoders_motors = EncoderMotors()
        self.encoders_motors.reset()

        # 计算近似半径
        self.radius1 = (self.rx + self.ry) / 2
        self.radius2 = (self.rx + self.ry) * 2

        self.dis_now = 0
        self.pos_start = np.array([0, 0, 0])
        # 车子整体前进的路程变量
        self.dis_traveled = 0
        # 车子起始位置相对世界坐标系
        self.odom_x = 0
        self.odom_y = 0
        self.odom_theta = 0
        self.speed_now = np.array([0, 0, 0])
        
        self.last_encoders = self.encoders_motors.get()
        self.odom_update_flag = True
        self.last_time = time.time()

        self.odom_thread = Thread(target=self.odom_update, args=())
        self.odom_thread.daemon = True
        self.odom_thread.start()
        time.sleep(0.2)

        # self.base_motors.set_speed(speeds)

    #从yaml文件中获取预先设定好的配置
    def get_yaml_config(self):
        while True:
            try:
                with open(self.yaml_path, 'r') as stream:
                    self.config = yaml.load(stream, Loader=yaml.FullLoader)
                # 轮胎直径
                self.wheel_diameter = self.config["wheel_diameter"]
                # 根据存储的轮胎直径获取轮胎的周长 
                self.wheel_perimeter = self.wheel_diameter * math.pi
                self.wheel_convert = MotoConvert(self.wheel_perimeter)
                self.rx = self.config["rx"]
                self.ry = self.config["ry"]
                break
            except Exception as e:
                # print(e)
                self.config = {
                    "wheel_diameter": 0.06,
                    "rx": 0.23,
                    "ry": 0.24,
                }
                with open(self.yaml_path, 'w') as stream:
                    yaml.dump(self.config, stream)

    #蜂鸣器控制
    def beep(self):
        self.buzzer.set(200, 10)

    #控制4个轮子
    def run4(self, *speeds):#[0,0,0,0]
        # print(speeds)
        dir = -1
        if isinstance(speeds[0], list):
            sp_tar = speeds[0]
        else:
            sp_tar = list(speeds)
        for i in range(4):
            dir = dir * -1
            sp_tar[i] = int(dir*self.wheel_convert.sp2virtual(sp_tar[i]))
        # print(sp_tar)
        self.base_moto.set(*sp_tar)

    def get_encoders(self):
        re_list = self.encoders_motors.get()
        if not re_list:
            return [0, 0, 0, 0]
        re_list[0] = re_list[0] * -1
        re_list[3] = re_list[3] * -1
        return re_list

    # 运动学解算
    def mecanum_inverse(self, vx, vy, vomega):
        speed_out = [0, 0, 0, 0]
        speed_out[0] = -(vx - vy - self.radius1 * vomega)
        speed_out[1] = -(vx + vy + self.radius1 * vomega)
        speed_out[2] = vx - vy + self.radius1 * vomega
        speed_out[3] = vx + vy - self.radius1 * vomega
        return speed_out

    def mecanum_wheel(self, speed_vx, speed_vy, speed_vomega):
        speeds = self.mecanum_inverse(speed_vx, speed_vy, speed_vomega)#[0,0,0,0]
        self.run4(speeds)

    def mecanum_forward(self, d_vect):
        car_dx = (d_vect[0] + d_vect[1] + d_vect[2] + d_vect[3]) / 4.0  # 车x方向位移速度
        car_dy = (0 - d_vect[0] + d_vect[1] - d_vect[2] + d_vect[3]) / 4.0;  # 车y方向位移速度
        car_domega = (0 - d_vect[0] + d_vect[1] - d_vect[2] + d_vect[3]) / self.radius2  # 车角速度
        return car_dx, car_dy, car_domega

    def odom_update(self):
        self.last_encoders = np.array(self.get_encoders())
        self.last_time = time.time()
        while True:
            if not self.odom_update_flag:
                break
            try:
                # 获取当前编码器值
                current_enc = np.array(self.get_encoders())
                d_enc = current_enc - self.last_encoders
                dt = time.time() - self.last_time
                if dt == 0:
                    dt = 0.05
                self.last_time = time.time()
                self.last_encoders = current_enc
                # 计算编码器值变化
                d_dis = self.wheel_convert.dis2true(d_enc)
                # 计算这个瞬间的位移并叠加
                self.dis_traveled += np.average(d_dis)
                # 计算当前位移角度变化
                car_dx, car_dy, car_domega = self.mecanum_forward(d_dis)
                self.speed_now = np.array([car_dx, car_dy, car_domega]) / dt
                self.dis_now += car_dx
                dx = car_dx * math.cos(self.odom_theta) - car_dy * math.sin(self.odom_theta)
                dy = car_dx * math.sin(self.odom_theta) + car_dy * math.cos(self.odom_theta)
                domega = car_domega
                self.odom_x += dx
                self.odom_y += dy
                self.odom_theta += domega
            except Exception as e:
                pass
            time.sleep(0.05)
    
    def get_odom(self):
        return [self.odom_x, self.odom_y, self.odom_theta]
    
    def get_dis_traveled(self):
        return self.dis_traveled
    
    def get_speed_closed(self):
        return self.speed_now
    
    def close(self):
        self.odom_update_flag = False

if __name__ == '__main__':
    car = MecanumBase()
    car.beep()
